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I. INTRODUCTION 


In any operational scenario of an underwater vehicle there exits a triple-nested 
sequence of mission accomplishment operations: Path planning, navigation, guidance, and 
autopilot design. The path planner takes information from charted obstacles and friendly 
or hostile environments and generates a smooth plan for the vehicle to follow. A certain 
level of feedback exists in this operation through the use of sonar beams in order to 
replan a path when uncharted objects are encountered or when the mission requirements 
have changed. Based on the desired vehicle positions and orientations at certain points, 
several classes of smooth paths containing sets of straight line segments, and circular arcs 
or cubic splines can be obtained [1]. Once a smooth path is generated, the navigator 
provides through a selected guidance law the appropriate vehicle heading commands 
which are in turn delivered by the autopilot. Line of sight guidance using a discrete series 
of way points was studied by Lienard in [2] using sliding mode heaving, propulsion, and 
depth keeping autopilots. The scheme demonstrated excellent stability and robustness 
characteristics, although the actual vehicle response was found to lag significantly the 
commanded straight line paths. The guidance and autopilot functions can be combined 
when the lateral deviation off the desired path is directly incorporated into the control law 
design. This leads to the development of a cross track error autopilot. Such schemes have 


been studied by Chism [3] and Hawkinson [4] for the single input and multiple input 


case, respectively. Cross track error autopilots provide more accurate path keeping 
response but they must be designed more carefully since they tend to be more dependent 
than heading controllers on an accurate description of the vehicle hydrodynamic 
characteristics. The main reason for this is the increase in the system dimensionality by 
one. Underwater vehicles operate in changing environments over a wide range of 
operating speeds and, therefore, a certain degree of uncertainty exists in the vehicle 
dynamic modeling. Cross track error autopilots also require accurate positional 
information updates at the same rate as heading and heading rate. 

For these reasons, in this work we go back to the case of a heading autopilot 
coupled with an orientation guidance law. The two main tasks on which we will 
concentrate are as follows: First, we must develop a way of establishing stability of the 
combined autopilot/guidance scheme for straight line commanded motions. Second, the 
actual vehicle response characteristics must be made to resemble the desired cross track 
error response with smooth transitions between consecutive straight line segment and with 
minimal path overshoot. A linear full state feedback control law is used to adjust the 
heading of the vehicle to any desired value, and a pure pursuit guidance law 1s used to 
provide the commanded heading for straight line motion. In this scheme the vehicle 
commanded heading equals the line of sight angle between the vehicle center and a target 
point moving on the desired path at a constant lookahead distance from the vehicle. This 
parallels the case studied in [5] except that in our case the existence of lateral and 
rotational dynamics add more complications to the problem. All computations are 


performed for the Swimmer Delivery Vehicle [6] for which a set of hydrodynamic 


characteristics and geometric properties is available. Problem formulation and equations 
of motion are presented in Section II. The analysis procedure is outlined in Section IJ, 
and simulation results are presented in the Section IV. Finally, conclusions and 


recommendations for further research are given in Section V. 


HW. DEVELOPMENT OF THE MATHEMATICAL MODEL 


A. EQUATIONS OF MOTION 
In a moving coordinate frame fixed at the vehicle center, Newton’s equations of 


motion for a rigid body in the horizontal plane are 


mvturt+xe-yor’) = Y, (2) 
ie: +mx (Vv +ur)-my-vr = NN, (2.2) 
where 


Vv = sway(lateral) velocity, 

r = yaw(angular) velocity, 

u = forward(surge) speed, 

Y = sway force, 

N = yaw moment, 

m = vehicle mass, 

I, = vehicle mass moment of inertia, 

(Xg,Yg) = coordinates of center of gravity. 

Expanding the force Y and moment N in added mass, damping, and drag terms, 


equations (2.1) and (2.2) are written as 


mV +ur +X Fr ey = fl Das FI *(Y,v+Y ur) 





: 3 (2.3) 
gle] *Y uv mu CA) ats, dx+P] *y,u°d, 
2 re Slee) 2 
iy +mx (Vv tur)-my vr = £1 Nr EI *(N.V +N ur) 
(2.4) 





+P pn iy -P ic, h(x) (v +ary’ xax+P] *y.u Od, 
2 Boy | v+xr | 2 
where 
p = water density, 
1 = vehicle length, 
& = mdder angle, 
h(x) = vehicle height distribution, 
Cp, = drag coefficient. 
The inertial position of the vehicle (x,y) and its heading angle y ( see Figure 1 ) 


are given by 


T= ep (2.5) 
xX = ucosy-vsiny , (2.6) 
y = usiny+ycosy . (27) 


Vv 


Y 
VEHICLE 
oO 
y 
TARGET (STEERING POINT) 
Xq 
[$$$ fir} 


Figure | Top view of the vehicle 


B. STATE SPACE EQUATIONS 
Choosing ( Y, v, r ) as the state vector, the linearized state space equations (2.3), 


(2.4), and (2.5) are written as 


" Se (2.8) 
vo = d,uvta,ur+bus , (Ze) 
r = a, uvea,urtbu’s . (2.10) 


The coefficients in equations (8), (9), and (10) are given by 


; (I_-0.5p/5N,)(0.5p!7y,)-(mX ,-0.5p/*y,)(0.5p/°N ) 
. (I -0.5p1°N.)(m-0.5p1 *y,)-(mX ,-0.5p1 “y,)(mX,,-0.5p1 4N,) 


(I, -0.5p/°N,)(-m+0.5pl *y,) -(mX ,-0.5pI 4y.)(-mX,,+0.5p/4N ) 
‘ (I -0.5p/°N.)(m-0.5pl *y,)-(mX ,-0.5pl 4y,)(mX ,-0.5p/4N,) 


(mX ,-0.5p1 *y,)(0.5p13N,)-(mX,,-0.5p! *N,)(-m +0.5p/2y ) 
. (I -0.5p1°N.)(m-0.5p1*y,)-(mX ,-0.5p! *y.)(mX,,-0.5p1 4N,) 


(mX ,-0.5p! “y.)(-mX,, +0.5p/“N_) -(mX,, -0.5pl “N.)(-m +0.5p/ *y_) 
% (I -0.5p/5N,)(m-0.5p12y,)-(mX,,-0.5pl “y.)(mX _,-0.5p1 4N,) 


(I. -0.5p!5N,)(0.5p! 2y,)-(mX,,-0.5p!‘y,)(0.5p/2N,) 
(I_-0.5p/°N.)(m-0.5pl *y,)-(mX _,-0.5pl “y,)(mX,,-0.5p1“N,) 


» = __X-0-5p! *y.)(0.5p/°N,)-(mX,, -0.5p/ 4N,)(0.5p/7y,) 
; (I -0.5p/5N.)(m-0.5p!*y.)-(mX ,-0.5p/*y.)\(mX,,-0.5p14N,) 


where 


= lateral hydrodynamic coefficients 


Ys: a BAe, ys ys 


N,,N.,N,,N.,N, = yaw hydrodynamic coefficients 


Equations (2.8), (2.9), and (2.10) describe the dynamics of the system with respect 


to small deviations around a nominal direction y = 0. 


C. PATH KEEPING DEVELOPMENT 


I. Heading control 


A linear full state feedback control law is of the form 


6 = kyrk,v+k,r (2.11) 
where k,, k, and k, are the three gains. 
From equation (2.8),(2.9),(2.10) and (2.11), the closed loop system is 
YWorzr (2.12) 
(2.139 


bu *k w+(a, uth urk,)v +(a,u+b u7k,)r 


y 


r = b,u?k, w+(a,utb,u7k,)v+(a,,u+b,u7k,)r (2.14) 


The characteristic equation of (2.12), (2.13) and (2.14) is 


OzX 0 1 
bu*k, a,urtbu’k,-A — a,,u+b,u"k, = QO 


2 O. 2, _ 
Palko eed, OE ke a, tO Kk, A 


M(a,,u+b,u7k, -A)(a,,u+b,u°k, -A)-(a,,u+b,u7k,)(a,,u+b,uk,)] 


iH) 
om 


-b u?k, (a,,u+b,u7k,)+b,u 7k (a, u+b,u7k,-A) 


Aa, ,4,,47+a,,b,u?k,—-Aa, u+b,a,,u°k, +b,b,u “kk, -b,u 7k, -Aa,u 
~Nb,u 7k, +02-a,,4,,U?—a,,b,u 2k, -a, bu *k,-b,b,u*k,k,) 


-b,a,,u°k,-b,b,u*k,k, +a, b,u?k, +b,b,u ‘kk, -Ab,u 7k, = 0 


3_ 2 2 2 2 3 3 
a (a, ,u+b,u°k, +a,,u+b,u k,)A aA CRI Mie? BT Ol ae OK? Bt 


-d,,4,,u?-a,,b,u°k,-a,,b,u*k,-b,u?k, )A+b,a, u*k, -b,a, uk, =- 0 (Za) 


2. Desired characteristic equation 


The 3rd order ITAE polynomial is defined by 


AF +1.750,A7+2.15@,A+0, = 0 (2.16) 


ul 


where 
t. = settling tume (dimensionless) 


From equations (2.15) and (12.6) we get 
“4 U-b UK, -d_ =D k= le (2.17) 


2 3 a 2 
A, Aaolt +a,,b,u k,+b,a,u Le a ,.4,,U 


-a,,b,u°>k,-a,,b,u?k,-b,u?k, = 2.15@p (2.18) 


b,a,,u>k,-b,a,,uek, = Wp (2.19) 


The system of equations (2.17), (2.18) and (2.19) can be solved for the three gains 


3 
Wo 


ac, 3 
bau ba, ,u 


UW (=) 750), a, ae) Ge b,)-b,u?(2.15@5-a-1 la,u*+a,a,,u*+b,u°k,) 


202 12721 
kK. = 


b,u(a,,b,-a,,b,)-b,u°(a,,b,-a,,b,) 


21° 1 


ee bu 212.15@,-u 2( Goad 445) Ost ake lal 75s, -a, ,u-a,,u)(a,,b,-a,,b,) 


b u°(a_ Oana (ano es) 


1272 
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3. Pure pursuit navigation 


For a pursuit navigation 


y. = 0 (2.20) 


where 
y. = commanded heading 


Referring to Figure 1, the line of sight angle o is defined by 


o = -tan?> 
x 2275 
where x, is the vehicle lookahead distance, and the control law (2.11) becomes 
& = k(y-y)+kvt+k,r (222) 
Using equations (2.20), (2.21) and (2.22) we get 
§ = k(wetan')+ky+kr 
iv a (2.23) 
The linearized equation (2.23) becomes 
§ = k(w+)+kythr 
iY = sie (2.24) 
The linearized equation for the lateral deviation y is obtained from (2.7) as 
yo= wyty (2.25) 
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Now the complete state vector is y, v, r and y , and the state space equations are written 


as 


bu 2k w+(a, u+b,u2k,v+(a,u+b,u2k,r+b wrk Ly 
x4 


Ss 
! 


~“. 
It 


bu*k wr(a,,urb,u7k,)v+(a,,utb,u7k,)r+b,u 2% Ay 


and the characteristic equation is 


0-A 0 I 0 


bu*k, a,urtbu7k,-A — aurbu’k, bu°k, 


2h 2 a, 2 
Dita a, ur+b,u°k, a,,u+b,u°k, Xr Dee 


u l 0 0O-A 
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(2.26) 


(2:27) 


(2.28) 


(2°29) 


(2.30) 


I. COMPUTER SIMULATION 


A. PROGRAMMING 


1. Program in MATRIX.X 

The MATRIX.X software is in VAX/VMS in the Mechanical Engineering 
Department. The program in MATRIX.X is written to find gains and poles of the system 
from the given inputs, forward speed (u), settling time (t,) and vehicle lookahead distance 
(x,). This is used to compute the eigenvalues of the complete system ( Equation (2.30), 
The four eigenvalues of the system are computed for the given values of u, t, and X,. 
For stable vehicle response, all four must be negative (or have negative real parts). If at 
least one is positive, the vehicle response will be unstable and convergence to the straight 
line path is not to be expected. In such a case, the parameters (in particular the lookahead 
distance must be changed) so that the vehicle is stable. A listing of this program is 


presented in Appendix A. 


2. Program in FORTRAN 
The first program in FORTRAN, is written to find distance along body fixed 
axis ( x-y ) from inputs, heading angle (y), perpendicular distance from vehicle to route 
(y), yaw rate (r), forward speed (u), settling time (t,) and vehicle lookahead distance (x,). 
A modified version of this simulation program is presented in Appendix B. The modified 
version 1s used to control the vehicle position in a general ( X-Y ) inertial system. The 


desired vehicle path is discretized into a series of straight line segments and the same 
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lookahead distance x, is used to regulate the vehicle deviation of each segment. Details 


of this modification are presented in the next paragraph. 


3. Graphics 
The GRAFSTAT graphic package is used to produce 2 dimensional graphs 


by using data from the simulation programs. 


B. DETAILS 


1. Poles and gains 
Calculate the system poles and gains for a given forward speed u and various 
combinations of t, and xy. Select t, and x, such that appropriate (sufficiently negative) 
poles and gains for the system are produced. This is verified by repeated simulations from 


step 2 that follows 


2. Distance in x-y axis 
Using the values of t, and x, from the previous step, the system response can 
be simulated. Unlike the control law design, the simulation is based on the full nonlinear 
equations of motion for the vehicle, (3), (4), (5) and (7). Typically, the initial conditions 


consist of nonzero values of the lateral deviation y and heading yw. 


3. Distance in X-Y axis 
A similar procedure is used to simulate the vehicle response in a general path 
in the X-Y plane, as shown in Figure 2. The perpendicular distance y from the vehicle 


to the desired route in the X-Y plane is used to compute the commanded heading angle 
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y.. The difference y-a, where & is the angle of the route with respect to the X-axis, ts 
used instead of the heading yw in the control law. The above computations are performed 
by appropriate coordinate rotations between the two coordinate systems. . 

When transiting from one straight line path to the next, the same lookahead distance 
Xx, is used for both segments. The vehicle switches to the next segment when it gets 
within a specified distance from the terminal way point. This distance 1s measured along 
the x-axis and for given t, and x,, and it should increase as the angle a for the next 
segment increases. Too high or too low values of this tuming distance result in path 
overshoot and undesirable oscillatory response. The optimum turning distance that allows 
for the smooth transition between consecutive straight line paths is established with the 
aid of the simulation program from step 2 as follows. 

For a fixed initial heading y, the initial deviation y is varied until the vehicle 
response is smooth and sufficiently fast with no path overshoot. The process is repeated 
for different initial conditions in y and a curve in y versus y is constructed. This is 
shown in Figure 3 and is the desirable turning distance versus turning angle curve. The 


actual curve is approximated by two straight lines which are used to initiate the turn in 


the simulation program. A copy of this simulation program is included in Appendix B. 
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Figure 2 Angles and axes 
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Figure 3 Tuming distance and tuming angle 
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IV. SIMULATION RESULTS 


The vehicle parameters used in the simulation are: 


xX = -—0:0G16 y = 0.0012 

x, = 0.0040 y, = 0.0550 

x, = 0.0200 y, = 0.0300 

X- = -U0Ore y, = -0.1000 

x, = 0.0530 y; = 0.0270 

X,, = 0.00173 Cp, = 0.3500 

Xx, =) 0.1008 W = 12000 lb. 

n = -0.0034 if = 174 ft. 
0 0012 pop = 1.94 slug/ft *. 
n = -0.0160 g = 32.2 ft/sec. 
n, = -0.0074 I = 10000 {om 
ne = -0.0130 v = 0.000847 =ft*./sec. 


The simulation begins by setting w= 5 degrees, x, = 2 vehicle lengths, t, = 5, r 
= 0, v = 0 ft./sec, u = 5 ft./sec. and y = 1. When the vehicle moves to a distance x = 20 
then the simulation stops. The route of vehicle and the rudder angle (6) that vehicle used 
during simulation are shown in Figure 4. The heading angle (yw) and commanded heading 


(y.) are shown in Figure 5. Yaw velocity and sway velocity are shown in Figure 6. 
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The values for t, and x, were selected based on the results of the previous chapter. 
From the figures it can be seen that the vehicle response is very fast with limited 
overshoot. This, of course, depends heavily on the initial conditions of the simulation. The 
actual heading angle converges rather rapidly, after the initial transients have died out, to 
the commanded heading angle. 

The second series of simulations was performed in order to assess the capability 
of the control and guidance law to change course and keep the new path. Simulation 
parameters were x, = 2,t, = 5 andu=5 as before. Initial conditions for the simulations 
were Ww = 0, r = 0 and v = 0 with the initial vehicle position at (X),Y,)) = (5,0) in the 
global reference frame. The first straight line segment is determined by the way points 
(5,0) and (25,0) and the second by (25,0) and (67.89,20). For this route the corresponding 
course change is 25 degrees. The results of this simulation are presented in Figure 7 
where along with the actual vehicle path, a side path at distance of 1 vehicle length off 
the desired path is shown. This corresponds to an arbitrary " safety path band " for the 
vehicle. The turning distance was fixed at 2 vehicle lengths throughout the simulations. 
From Figure 7 it can be seen that the vehicle turns to the new course smoothly with no 
path overshoot. When the second route changes to (25,0) and (41.78,20) which 
corresponds to 50 degrees course change, the results of Figure 8 show that a path 
overshoot occurs although it is yet not serious enough according to the artificial safety 
criterion described above. However, when the second route changes to (25,0) and 
(30.36,20) which corresponds to 75 degrees course change, significant vehicle oscillatory 


response and side path overshoot occurs, as demonstrated in Figure 9. 
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The above simulations demonstrate the need for adjustable tuming distance; 
although path accuracy is obtained regardless of the value of the turning distance, the 
transient response during course change is not always within some predetermined safety 
bounds. For this reason we employ the built-in turning distance versus turning angle 
relationship shown in Figure 3 and repeat the simulations for the aforementioned three 
course changes. The results are shown in Figures 10, 11 and 12, where it can be seen that 
the vehicle response is now satisfactory for both course keeping and course changing. 

Finally, the last simulation test was performed in order to establish the capabilities 
of the scheme to follow a general path in the honzontal plane. For demonstration 
purposes the path was assumed to consist of the way points (0,10), (5,10), (25,0), (35,20), 
(50,20), (70,10), (50,-10) and (30,-10). Straight line segments were assumed as the desired 
paths between consecutive way points. The same simulation parameters were used as in 
the previous runs. The results of this sumulation are presented in Figures 13 for adjustable 
turning distance, and 14 for the fixed tuming distance case. It can be clearly seen that 
when the turning distance is function of turning angle, the scheme achieves excellent path 


keeping characteristics with smooth course changes and minimal path overshoot. 
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Figure 4 Pursuit navigation 


21 


0 


20 


3 : 
: : 
: : 
; 3 
ne cecces descecece@enccccopeaccsons Suacceschtenccsantiscscucesbencceces 
: : 
an a || a oe ene ee A a 
: : - 
ss neeen Focngocnioe SOOO JECEEOOOS Resgsscs Qscctnns- Ocsssccecheeccoces 
a eee OR tol 2 eR ee oO 
: . : : = - as 
‘ 2 + ot ‘ e > 
Jecucccsicsececcestedmeces: deseerees pecstnasssascesereccwisces=kaia) Uf) 
eecrece ficsscnetiQcereecs-Pecccssesbeceseersdsssseen: ® savers sone 


: 5 oO 
v OYA 0 OZ Ov-— 
(S33N93G) JIONV ONIGVSH 


20 


+. eee 2 eee | ee ee 
TT Terre TELE Irie PIT eee Pree Pir Sr 
oO ie On: 6) - ane | Ce: ele 


. . ° e 
COPE ree Cee ire? SET ieee eee Piri i ee a 


10 
TIME (DIMENSIONLESS) 





. ma oO 
Ov 02 0 | 0 Area Or— 
(S33N93G) ONIGVSH GIONVWWOD 


Figure 5 Pursuit navigation 


Pape 





15 20 


10 
TIME (DIMENSIONLESS) 


Figure 6 Pursuit navigation 


23 


Y POSITION 


TURNING ANGLE 25 DEGREES 
TURNING DISTANCE CONSTANT 


SIDE PATH 





20 24 28 52 
X POSITION 


Figure 7 Path control 
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Figure 8 Path control 
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Figure 9 Path control 
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Figure 10 Path control 
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Figure 11 Path control 
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Figure 13 Pure pursuit navigation 
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V. SUMMARY AND CONCLUSIONS 


The principal conclusions of this work can be summarized as follows: 
1. Orientation control law can be used in order to provide accurate vehicle path keeping 
when combined with an appropriate guidance scheme. 
2. Pure pursuit guidance was found to work very well for the autonomous underwater 
vehicle case and its simplicity make it a very attractive alternative to cross track error 
schemes. 
3. A built-in turning distance versus course change relationship can be utilized to initiate 
the turn at the appropriate time in order to avoid path overshoot and achieve smooth path 
transitions. 
4. It is expected that the added robustness that heading schemes naturally enjoy will aid 
in maintaining stability in cases where incomplete and inaccurate vehicle dynamic 
descriptions are available. 

Some recommendations for further research are as follows: 
1. Comparative studies must be performed with other orientation guidance schemes such 
as proportional navigation and also with velocity guidance laws in order to ensure that 


the best technique is ultimately employed. 


a2 


2. Similar studies must be performed in the vertical plane. Combined with the horizontal 
plane techniques developed in this work and with propulsion control they could be 


utilized to provide accurate trajectory following in 3-D space. 
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APPENDIX A. 


7 425: 

inquire xd l 

inquire tc. 

inquire u 

xd=xd 1*1; 

aall=-0.045380; 

aal2=-0.351190; 

aa21=-0.002795; 

aa22=-0.095680; 

bbl= 0.011432; 

bb2=-0.004273; 

OMEGA=(10.0*U)/(TC*L); 

AD1=1.75*OMEGA; 

AD2=2.15*OMEGA**2; 

AD3=OMEGA** 3; 

Al=BB1*U*U; 

B1=BB2*U*U; 

C1=-AD1-(AA11+4+AA22)*U; 

A2=(BB1*AA22-BB2*AA12) *U**3; 

B2=(BB2*AA11-BB1*AA21 ) *U**3; 

K1=AD3/( (BB2*AA11-BB1*AA21 ) *U**3); 

C2=AD2-(AA11*AA22-AA12*AA21 ) *U**24BB2*U*U*K1; 

K2=(C1*B2-C2*Bl)/(A1*B2-A2*B1); 

K3=(C2*A1-C1*A2)/(A1*B2-A2*Bl1); 

all=0; 

al2=0; 

al3=1; 

al4=0; 

a21l=bbl*u*turkl1; 

a22=aall*utbbl*tu*ur*tk2; 

a23=aal2*u+bbl *u*u*k3; 

a24=bbl*u*u*kl/xd; 

a3l=bb2*u*u*kl; 

a32=aa2l*utbb2*ur*tur*k2; 

a33=aa22*utbb2*ur*urk3; 

a34=bb2*u*ur*kl/xd; 

a4l=u; 

a42=1; 

a43=0; 

a44=0; 

a={all,al2,a13,a14;a21,a22,a23,a24; 
a31,a32,a33,a34;a41,a42,a43,a44]; 

eig(a) 
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APPENDIX B. 


PROGRAM SUB.FOR 


PROUTTICHAI SUWANDEE 
NAVAL POSTGRADUATE SCHOOL 
MARCH 1991 


AUV LINE-OF-SIGHT NAVIGATION AND CONTROL 
VARIABLE GAINS INTERNALLY COMPUTED 


REAL L,MASS,NRDOT,NVDOT,NR,NV,NDR 
REAL I1Z,NU,LLL,NSL,KI,K2K3 
DIMENSION X(9),HH(9),VEC1(9),VEC2(9),TT( 1000) (yy (6 oe 


*ALPHA( 10), X2¢( 10)" ¥2¢ 1) 


1 


1 


df 


i 


LONGITUDINAL HYDRODYNAMIC COEFFICIENTS 


PARAMETER( XRR=4.E-3,XUDOT=-7.6E-3,XVR=2.E-2,XRDR=-1.E-3, 
XVV=5.3E-2,XVDR=1.7E-3,XDRDR=-1.E-2) 


LATERAL HYDRODYNAMIC COEFFICIENTS 


PARAMETER ( YRDOT=1.2E-3,YVDOT=-5.5E-2,YR=3.E-2,YV=-1.E-1, 
YDR=2./7E-2,CDY=3.5E-1) 


YAW HYDRODYNAMIC COEFFICIENTS 


PARAMETER (NRDOT=-3.4E-3,NVDOT=1.2E-3,NR=-1.6E-2,NV=-7.4E-3, 
NDR=-1.3E-2) 


MASS CHARACTERISTICS OF THE FEOODEDS EAteee 


PARAMETER (WEIGHT=12000.,XG = 0.,12=10000.,L=17.4,RHO=1.94, 
G=32.2,NU=8.47E-4) 


OPEN (10,FILE="SUB.1IN’ ,STATUS="Orp 
OPEN (11,FILE="SUB.OUT’ (| STATUS=" Op) 


READ (10,*) TARGET 

READ (10,*) TSIM,DELTA,IPRNT 
READ (10,*) PSI,R 

READ (10,%*) U 

READ (10,*) TC,VC 


NUMBER OF POSITION OF ROUTE AND POSITION OF VEHICLE 


READ (10,*) N,XO,YO 
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CYOrQ 


aAa-aAN 


aQaAnN aqaAn aA-AaAN 


aAaaAA 


aqaaAnN 


30 


E@sitTION OF SROUTE IN X-¥ AXIS 


DO 30 I=1,N 

Peep e(l0,*) KZ(1),YZer) 
CONTINUE 
TARGET=TARGET*L 
TWOPI =8.0*ATAN(1.0) 
PI =0.5*TWOPI 

DO 999 M=1,N-1 
PSI1=0 

YTURN=0 

XYTURN=0 
PSI=PSI*P1I/180.0 


MOVE ORIGINE OF AXIS TO THE FIRST POINT OF THE ROUTE 


XF=XZ(M+1)-XZ(M) 

moe XF .FO.0) XF=0.0000001 
YF=YZ(M+1)-YZ(M) 
XO=XO-XZ(M) 

met xO.EO.0) XO=0.0000001 
YO=YO-YZ(M) 


ANGLE OF POSITION OF VEHICLE 


ALPHAO=ATAN (ABS (YO/XO) ) 

oe (¥O.GT.0) .AND.(XO.LT,; 
mp  (@yO.LE.0)5AND.(XO.GT. 
a He YO. LES Oye AND. (XO°.LT. 


ALPHAQ=PI-ALPHAO 
ALPHA0=2*PI-ALPHAO 
ALPHA0=PI+ALPHAO 


OO © 


ANGLE OF ROUTE 


ALPHA1=ATAN(ABS(YF/XF) ) 

ie ( (YRaGT. 0) AND. (XF.UT.0 
fme( (VE.LE,0),AND. (XF.GT.0 
me ((YF.LE.0)- AND. (XF.LT.0 


ALPHA1=PI-ALPHA] 
ALPHA1=2* PI-ALPHA1 
ALPHA1=PI+ALPHA1 


See ee 
wee ee Se” 


ANGLE BETWEEN VEHICLE AND ROUTE 
BETA=(ALPHAO-ALPHAI1 ) 

DISTANCE FROM ORIGINE TO VEHICLE 
peL= Owe Z+yYO**2)**0.5 


PROJECTED DISTANCE FROM ORIGINE TO VEHICLE ON THE ROUTE 


XS=RT*COS( BETA) 
XC=(XS)*COS(ALPHAI ) 
YC=(XS)*SIN(ALPHAI ) 


of 


Or Oa 1a @ 


CYrGa) 


a Ge 


Aaa 


PERPENDICULAR DISTANCE OF VEHICLE TO X=AXI5S 
YPOS=RT*SIN( BETA) 
TOTAL DISTANCE ON THE ROUTE 


TXD=(XF**2+YF**2)**Q.5 
XD=ABS(TXD-XS ) 


HEADING ANGLE 


IF(M.EQ.1) PSI=PSI-ALPHA] 

IF (PSI.GT.PI) PSI=PSI-2*PI 
IF (PSI.LT.-PI) PSI=PSI+2*PI 
IF (MEQ. N=1 )¥GO TO as 


NEXT HEADING ANGLE 


DUMY=YZ(M+2)-YZ(M+1) 

DUMX=XZ(M+2)-XZ(M+1) 

IF (DUMX.EQ.0) DUMX=0.0000001 
ALP2=ATAN(ABS(DUMY/DUMX) ) 

IF ((DUMY.GT.0).AND.(DUMX.LT.0)) ALP2Z=@PI-ALP2 
IF ((DUMY.LE.0).AND.(DUMX.GT.0)) ALP2=?*PI-ALP2 
IF ((DUMY.LE.0).AND.(DUMX.LT.0)) ALP2=EI+ALP2 
PSI1=ALPHA1-ALP2 

IF (PSI1.GT.PI) PSI1=PSI1-2*PI 

IF (PSPPOLT. 21) PSilipomi een 
PSI1=PSI1*180/PI 


TURNING DISTANCE 


IF(ABS(PSI1).LE.45) YTURN=ABS(PSiiW725) 
IF(ABS(PSI1).GT.45) YTURN=(ABS(PSI1)-45)*0.3/5+1.8 
XYTURN=YTURN/ABS(SIN(PSI1*PI/180))-0.2 
IF(PSITIEQsS0)7 27 TURN=(eao 


We =U 
OMEGA=(10.0*U)/(TC*L) 
ADIL <=1.75*OMEGA 

AD2 =2.15*OMEGA**2 
AD3 =OMEGA** 3 


PISIM =TSIM/DELTA 
ISIM =PISIM 

ECHO =1.0/DELTA 
IECHO =IPRNT*20 


YAW =0.0 
SWAY =0.0 
V =0.0 
DR = 29 
R =R*PI7Vo0re 
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AAN 


ISTART=1 
XPOS =0.0 
YPOS *#YPOS*L 


DEFINE THE LENGTH X AND HEIGHT HH TERMS FOR THE INTEGRATION 


me 


X(1) = -105.9/712. 
X(2) = -99.3/12. 
X(3) = -87.3/12. 
X(4) = -66.3/12. 
X(5) = TZ ug « 
X(6) = 83.2/12. 
X{7) = 91.2712: 
X(8) = 99.2/12. 
mC = 103.2/128 
HH(1) = 0.00/12. 
HH(2) = 8.24/12. 
HH(3) = 19.76/12. 
HH(4) = 29.36/12. 
Poo ye= 31.85/12. 
HH(6) = 27.84/12. 
fib ) = 21.44/12. 
HH(8) = 12.00/12. 
HH(9) = 0.00/12. 


MASS = WEIGHT/G 


P1=MASS-0.5*RHO*L**3*XUDOT 
P3=MASS-0.5*RHO*L**3*YVDOT 
P4=MASS*XG-0.5*RHO*L**4* YRDOT 
P5=1Z-0.5*RHO*L**5*NRDOT 
P6=MASS*XG-0.5*RHO*L**4*NVDOT 
D =P5*P3-P4*P6 


AA11=(P5*0.5*RHO*L*L*YV—P4*0.5*RHO*L**3*NV)/D 
AA12=(P5*(-MASS+0.5*RHO*L**3*YR)—P4* (-MASS*XG 

1 +0.5*RHO*L**4*NR))/D 

AA2Z1=(P3*0.5*RHO*L** 3*NV—-P6*0.5*RHO*L*L*YV)/D 
AA2Z2=(P3*(-MASS*XG+0.5*RHO*L**4*NR)-P6*(-MASS 

Li +0.5*RHO*L**3*YR))/D 

BB1=(P5*0.5*RHO*L**2* YDR-P4*0.5*RHO*L**3*NDR) /D 
BB2=(P3*0.5*RHO*L**3*NDR-P6*0.5*RHO*L**2* YDR) /D 


Al =BB1*U*U 
Bl =BB2*U*U 


Cl =-AD1-(AA11+AA22)*U 
A2 =(-AA12*BB2+AA22*BB1 ) *U** 3 
B2 =(-AA21*BB1+AA11*BB2) *U**3 


K1=AD3/( (BB2*AA11-BB1*AA21 ) *U** 3) 

C2 =AD2-(-AA12*AA21+AA11*AA22 ) *U**24+BB2*U*U*K1 
K2=(C1*B2-C2*B1)/(A1*B2-A2*B1 ) 
K3=(C2*A1-C1*A2)/(A1*B2-A2*Bl1 ) 
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OOS 


CGI 


Aaa 


CAO 


600 


J 


=0 


IJ=0 
JE=0 
OFF=0 


DRHAT=0 .0 
DRBAR=0.0 


SIMULATION BEGINS 


DO 100 I=1,I1SIM 


CALCULATE THE DRAG FORCE, INTEGRATE THE DRAG OVER THE VEHICLE 


DO 600 K=1,9 
UCF=V+X(K)*R 
SGN=1.0 
IF (UCF.LT.0.0) SGN=—1.0 
VEC1(K)=HH(K) *UCF*UCF*SGN 
VEC2(K)=HH(K)*UCF*UCF*SGN*X(K) 
CONTINUE 
CALL TRAP(9,VEC1,X, SWAY) 
CALL TRAP(9,VEC2,X, YAW) 
SWAY=-0.5*RHO*CDY*SWAY 
YAW =-0.5*RHO*CDY*YAW 


FORCE EQUATIONS 


FP2 = -MASS*U*R+0.5*RHO*L**3*YR*U*R+0.5* RHO*L*L* ( 
YV*U*V+YDR*U*U*DR) +SWAY 
FP3 = -MASS*XG*U*R+0.5*RHO*L**4*NR*U*R+0. 5 *RHO*L* * 3 * 


(NV*U*V+NDR*U*U*DR) +YAW 


VDOT =(PS5*FP2-P4*FP3)/(P5*P3-P4*P6) 
RDOT =(P6*FP2-P3*FP3)/(P4*P6-P3*P5) 
PSIDOT=R 

YDOT =U*SIN(PSI)+V*COS(PSI)+VC 

XDOT =U*COS(PSI)-V*SIN( PSI) 


FIRST ORDER INTEGRATION 
PSI =PSI +DELTA*PSIDOT 
V =V +DELTA* VDOT 
R =R +DELTA*RDOT 
XPOS=XPOS+DELTA* XDOT 
YPOS=YPOS+DELTA* YDOT 


YCTE=YPOS 
XAWAY=(XPOS-XD*L) 


IF ((XAWAY).GE.-(XYTURN*L)) OFF=1 
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QaAN 


aAaAN 


aAaAaAYN 


a9 


100 
500 


SSS, 


RUDDER INPUT CALCULATION 


YA=ABS(YPOS) 
HDM=ATAN( (YPOS) /(-TARGET) ) 
DR=K1*(PSI-HDM)+K2*V4+K3*R 


IF (DR.GT. 0.4) DR= 0.4 
IF (DR.LE.-0.4) DR=-0.4 


PRINT RESULTS 


JE=JE+1 

IF (JE.NE.IECHO) GO TO 99 

WRITE (*,*) ' XAWAY =’, XAWAY/L 
a0 

J=J+1 

IF (J.NE.IPRNT) GO TO 100 
IJ=IJ+1 

TIME=I*DELTA 

XP=XPOS/L 

YP=YPOS/L 
XI=XZ(M)+XC+XP*COS(-ALPHA1] )+YP*SIN(-ALPHAI ) 
YI=YZ(M)+YC+YP*COS(-ALPHAI]1 )-XP*SIN(-ALPHAI ) 
WRITE (12,*) 421,Yi 

J=0 

me (OFF .EQ21) GO TO 500 
CONTINUE 

PSI=PSI1l 

XO=XI 

YO=YI 

CONTINUE 

STOP 

END 


SUBROUTINE TRAP(N,A,B,O0OUT) 
NUMERICAL INTEGRATION ROUTINE USING THE TRAPEZOIDAL RULE 


DIMENSION A(1),B(1) 

N1=N-1 

OUT=0.0 

DO 1 I=1,N1l 
OUT1=0.5*(A(1I)+A(I4+1))*(B(I+1)-B(I)) 
OUT =OUT+OUT1 

CONTINUE 

RETURN 

END 
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